// For bluetooth communication
#ifdef	WIN32
#include <WinSock2.h>
#include <ws2bth.h>
#define	AF_BLUETOOTH	AF_BTH
#define	BTPROTO_RFCOMM	BTHPROTO_RFCOMM
#else
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#endif

#include <stdexcept>
using std::runtime_error;

#include <iostream>
using std::cout;

//#include <cstring>
#include "epuck_impl.hpp"

namespace
{
	bool debug = false;
}

namespace gui
{
#ifdef	WIN32
	size_t read(SOCKET s, void* buf, size_t count)
	{ return recv( s, (char *)buf, count, 0 ); }
	size_t write(SOCKET s, const void* buf, size_t count)
	{ return send( s, (const char *)buf, count, 0 ); }
	int close( SOCKET s ) { return closesocket( s ); }
#endif

	EpuckImpl::EpuckImpl(const EpuckID id)
	: epuck(id)
	{
		const char	*macAddress=0;
		switch( epuck )
		{
			case Epuck1433: macAddress = "10:00:E8:52:BF:35"; break;
			case Epuck1031: macAddress = "10:00:E8:52:BC:49"; break;
			case Epuck1033: macAddress = "10:00:E8:52:C7:63"; break;
			case Epuck2386: macAddress = "10:00:E8:AD:79:FC"; break;
			case Epuck2477: macAddress = "10:00:E8:AD:79:BA"; break;
			case Epuck2534: macAddress = "10:00:E8:AD:75:8C"; break;
			case Epuck2539: macAddress = "10:00:E8:AD:75:A0"; break;
			default: throw runtime_error("Internal Error!");
		}	
		switch( epuck )
		{
			case Epuck1031:
			case Epuck1033:
			case Epuck1433:	helptextlength = 22; break;
			case Epuck2386:
			case Epuck2534:
			case Epuck2539: helptextlength = 25; break;
			case Epuck2477: helptextlength = 24; break;
			default: throw runtime_error("length of Help unknown");
		}
		
#ifdef	WIN32
		WORD	wVersionRequested = MAKEWORD( 2, 0 );
		WSADATA	wsaData;
		if( WSAStartup( wVersionRequested, &wsaData ) )
			throw runtime_error("windows sockets barfed");
#endif
		sock = socket( AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM );
		if( sock == -1 )
			throw runtime_error("Bluetooth socket creation failed");

// C++ type casts, read doc
#ifdef	WIN32
		SOCKADDR_BTH	loc_addr;
		wchar_t	*ma = new wchar_t[strlen(macAddress)+1];
		wsprintf( ma, L"%S", macAddress);
		int	socksize = sizeof(SOCKADDR_BTH);
		WSAStringToAddress( ma, AF_BTH, NULL, (SOCKADDR *)&loc_addr,
				&socksize);
		loc_addr.port = 1 & 0xff;
#else
		sockaddr_rc	loc_addr;
		str2ba( macAddress, &loc_addr.rc_bdaddr );
		loc_addr.rc_family = AF_BLUETOOTH;
		loc_addr.rc_channel = 1;
#endif
		if( connect(sock, (struct sockaddr *)&loc_addr,	// ?
				sizeof(loc_addr)) != 0)
		{
#ifdef WIN32
			cout << "WSA Error: " <<  WSAGetLastError() << "\n";
#endif
			throw runtime_error("rfcomm layer connection failed");
		}
		if( debug ) cout << "bluetooth connection established\n";
		
		/* Initially, the robot answers with "z,Command not found\n"
		 * and even with cryptic rubbish, so lets skip these lines
		 * directly at startup. The actual command to sent for that
		 * purpose does not play a role. */
		sendCommand("A\n", buffer, sizeof(buffer), 1);
		if( debug ) cout << buffer;
		sendCommand("A\n", buffer, sizeof(buffer), 1);
		if( debug ) cout << buffer;
		sendCommand("A\n", buffer, sizeof(buffer), 1);
		if( debug ) cout << buffer;
		if( debug ) cout << "garbage read\n";
	}

	EpuckImpl::~EpuckImpl(){ close(sock); }

	int EpuckImpl::readline( char *begin, int l )
	{
		int offs = 0;
		while ((offs < l) && (read( sock, &begin[offs], 1 ) == 1) )
		{
			if( begin[offs++] == '\n' )
				return offs;
		}
	
		// Error!
		return 0;
	}

	void EpuckImpl::sendCommand( const char *cmd, char *answer,
					int maxAnswerLength, int numAnswerLines)
	{
		int	len = strlen(cmd);
		if( write( sock, cmd, len) < len )
			throw runtime_error("Bluetooth sending failed");
		if( debug ) cout << "wrote " << cmd;
		int	offs = 0;
		for( int i = 0; i < numAnswerLines; i++ )
		{
			int tmp = readline(&answer[offs], maxAnswerLength-offs);
			if( tmp == 0 )
				throw runtime_error("Bluetooth receiving failed");
			offs += tmp;
			if( debug ) cout << "read line\n";
		}
		answer[offs] = '\0';
	}

	bool EpuckImpl::testBuffer( char const *s )
	{
		int	i = 0;
		while( *s != 0 )
			if( buffer[i++] != *s++ )
				return false;
		return true;
	}
	
	Int3 EpuckImpl::getAccelerometer()
	{
		Int3 result = {{0,0,0}};
		sendCommand("A\n", buffer, sizeof(buffer), 1);
		if( sscanf(buffer, "a,%d,%d,%d\r\n", &result.data[0],
				&result.data[1], &result.data[2]) != 3 )
			throw runtime_error("Reading accelerometer failed");
		return result;
	}

	void EpuckImpl::setBodyLED(LEDState state)
	{
		sprintf(buffer, "B,%d\n", state);
		if( debug ) cout << "setBodyLED " << buffer;
		sendCommand(buffer, buffer, sizeof(buffer),1);
		if( debug ) cout << "setBodyLED " << buffer;
		if( !testBuffer( "b\r\n" ) )
			throw runtime_error("Setting body led failed");
	}

	int EpuckImpl::getSelectorPosition()
	{
		int result = 0;
		sendCommand("C\n", buffer, sizeof(buffer), 1);
		if( debug ) cout << "setBodyLED " << buffer;
		if (sscanf(buffer, "c,%d\r\n", &result) != 1)
			throw runtime_error("Reading selector failed");
		return result;
	}

	void EpuckImpl::setMotorSpeed(int left, int right)
	{
		sprintf(buffer, "D,%d,%d\n", left, right);
		sendCommand(buffer, buffer, sizeof(buffer),1);
		if( debug ) cout << buffer;
		if( !testBuffer( "d\r\n" ) )
			throw runtime_error("Setting motor speed failed");
	}

	Int2 EpuckImpl::getMotorSpeed()
	{
		Int2 result = {{0,0}};
		sendCommand("E\n", buffer, sizeof(buffer), 1);
		if(sscanf(buffer, "e,%d,%d\r\n", &result.data[0],
				&result.data[1]) != 2)
			throw runtime_error("Reading motor speed failed");
		return result;
	}

	void EpuckImpl::setFrontLED(LEDState state)
	{
		sprintf(buffer, "F,%d\n", state);
		sendCommand(buffer, buffer, sizeof(buffer),1);
		if( debug ) cout << buffer;
		if( !testBuffer( "f\r\n" ) )
			throw runtime_error("Setting front led failed");
	}

	string EpuckImpl::getHelpText()
	{
		sendCommand("H\n", buffer, sizeof(buffer), helptextlength);
		return string(buffer);
	}

	EpuckImage &EpuckImpl::cameraShot()
	{
		buffer[0] = -'I';
		buffer[1] = '\0';

		if( write(sock, buffer, 2) < 2 )
			throw runtime_error("Bluetooth sending failed");

		// 3 byte header followed by raw 40x40 rgb565 image
		for (int l = 0; l < 3203;)
		{
			int tmp =read(sock, buffer+l, sizeof(buffer)-l);
			if (tmp < 1)
				throw runtime_error("Bluetooth "
					"receiving failed");
			l += tmp;	
		}

		// Sanity check: Format must be Mode = 1, Width = 40,
		//  Height = 40
		if(buffer[0] != 1 || buffer[1] != 40 || buffer[2] != 40)
			throw runtime_error("Weird epuck camera "
					"settings. Reset epuck!\n");
		EpuckImage *image = new EpuckImage( &buffer[3] );
		return *image;
	}

	void EpuckImpl::calibrateProximitySensors()
	{
		sendCommand("K\n", buffer, sizeof(buffer),2);
		const char *const str = "k, Starting calibration -"
			" Remove any object in sensors range\r\n"
			"k, Calibration finished\r\n";
		if( debug ) cout << "calProx: " << buffer;
		if( !testBuffer( str ) )
			throw runtime_error("Triggering calibration failed");
	}

	void EpuckImpl::setLED(int number, LEDState state)
	{
		// Work around minor bug in epuck's sercom code
		if( state == Toggle && (number < 0 || number > 7) )
		{
			for (int i = 0; i < 8; i++)
				setLED(i, Toggle);
		}
		else
		{
			sprintf(buffer, "L,%d,%d\n", number, state);
			sendCommand(buffer, buffer, sizeof(buffer),1);
			if( debug ) cout << "ledProx: " << buffer;
			if( !testBuffer( "l\r\n" ) )
				throw runtime_error("Setting ring led failed");
		}
	}

	Int8 EpuckImpl::getProximitySensors()
	{
		Int8 result = {{0,0,0,0,0,0,0,0}};
		sendCommand("N\n", buffer, sizeof(buffer), 1);
		if (sscanf(buffer, "n,%d,%d,%d,%d,%d,%d,%d,%d\r\n", 
				&result.data[0], &result.data[1],
				&result.data[2], &result.data[3],
				&result.data[4], &result.data[5],
				&result.data[6], &result.data[7]) != 8)
			throw runtime_error("Reading proximity sensors failed");
		return result;
	}

	Int8 EpuckImpl::getLightSensors()
	{
		Int8 result = {{0,0,0,0,0,0,0,0}};
		sendCommand("O\n", buffer, sizeof(buffer), 1);
		if (sscanf(buffer, "o,%d,%d,%d,%d,%d,%d,%d,%d\r\n", 
				&result.data[0], &result.data[1],
				&result.data[2], &result.data[3],
				&result.data[4], &result.data[5],
				&result.data[6], &result.data[7]) != 8)
			throw runtime_error("Reading light sensors failed");
		return result;
	}

	void EpuckImpl::setMotorPosition(int left, int right)
	{
		sprintf(buffer, "P,%d,%d\n", left, right);
		sendCommand(buffer, buffer, sizeof(buffer),1);
		if( !testBuffer( "p\r\n" ) )
			throw runtime_error("Setting motor position failed");
	}

	Int2 EpuckImpl::getMotorPosition()
	{
		Int2 result = {{0,0}};
		sendCommand("Q\n", buffer, sizeof(buffer), 1);
		if (sscanf(buffer, "q,%d,%d\r\n", &result.data[0],
				&result.data[1]) != 2)
			throw runtime_error("Reading motor position failed");
		return result;
	}

	void EpuckImpl::stopRobot()
	{
		sendCommand( "S\n", buffer, sizeof(buffer), 1);
		if( debug ) cout << "wrote " << buffer;
		if( !testBuffer( "s\r\n" ) )
			throw runtime_error("Stopping robot failed");
	}

	void EpuckImpl::playSound(int sampleNo)
	{
		sprintf(buffer, "T,%d\n", sampleNo);
		sendCommand(buffer, buffer, sizeof(buffer),1);
		if( !testBuffer( "t\r\n" ) )
			throw runtime_error("Playing sound sample failed");
	}

	Int3 EpuckImpl::getMicrophones()
	{
		Int3 result = {{0,0,0}};
		sendCommand("U\n", buffer, sizeof(buffer), 1);
		if (sscanf(buffer, "u,%d,%d,%d\r\n", &result.data[0],
				&result.data[1], &result.data[2]) != 3)
			throw runtime_error("Reading microphones failed");
		return result;
	}

	string EpuckImpl::getVersion()
	{
		sendCommand("V\n", buffer, sizeof(buffer), 2);
		if( debug ) cout << "wrote " << buffer;
		if (buffer[0] != 'v' || buffer[1] != ',')
		if( !testBuffer( "v," ) )
			throw runtime_error("Reading version failed");
		return string(&buffer[2]);
	}
}

